#include "ustc_comdef.h"
#include "i2c_init.h"
#include "sys.h"
//#include "test_comm.h"
#if 1
unsigned int device_address = 0x60;
void delay_new(unsigned int i)
{
	i*=2;
	while(i) i --;
}

void mercury_power(unsigned int value)//GPE4
{
	unsigned int i = 100000;
	IO_OUTWM(PCONFE,MERCUEY_POWRER_MASK,MERCURY_POWER_VALUE);
	IO_OUTWM(PDATE,MERCURY_POWER,value<<4);
	delay_new(i);
}


void mercury_reset(unsigned int value)//GPE5
{
	unsigned int i = 100000;
	IO_OUTWM(PCONFE,MERCUEY_RESET_MASK,MERCURY_RESET_VALUE);
	IO_OUTWM(PDATE,MERCURY_RESET,value<<5);
	delay_new(i);
}

void vrover_reset(unsigned int value)//GPG6
{
	unsigned int i = 100000;
	IO_OUTWM(PCONFG,VROVER_RESET_MASK,VROVER_RESET_VALUE);
	IO_OUTWM(PDATG,VROVER_RESET,value<<6);
	delay_new(i);
}

void sensor_mclk(void)
{
	UINT8 val;

	val = 0x3;

	IO_OUTWM(0x1d80000,0xFFFFF,0x38082);
	IO_OUTWM(PCONFE,0x03,val);
}

void sensor_reset(unsigned int value)//GPC7
{	
	IO_OUTWM(PCONFC,0xC000,0x4000);
	IO_OUTWM(PDATC,0x80,value<<7);
	delay_new(10000);
}

void sensor_powerdown(int value)//GPC3
{
	IO_OUTWM(PCONFC,0xC0,0x40);
	IO_OUTWM(PDATC,0x8,value<<3);
	delay_new(10000);
}


void i2c_config_clk(unsigned int value)//GPC9
{
	if(value == 0)
	{
		IO_OUTWM(PCONFC,0xc0000,0x0);
	}else{		
		IO_OUTWM(PCONFC,0xc0000,0x40000);
	}

	IO_OUTWM(PUPC,0x200,0x0);
}

void i2c_config_sda(unsigned int value)//GPC5
{
	if(value == 0)
	{
		IO_OUTWM(PCONFC,0xc00,0x0);		
	}else{
		IO_OUTWM(PCONFC,0xc00,0x400);		
	}

	IO_OUTWM(PUPC,0x20,0x0);
}

void i2c_set_scl(unsigned int value)//GPC9
{

	unsigned int i = 100;
	IO_OUTWM(PDATC,0x200,value<<9);		
	delay_new(i);
}

void i2c_set_sda(unsigned int value)//GPC5
{
	unsigned int i = 100;
	IO_OUTWM(PDATC,0x20,value<<5);
	delay_new(i);
}






/*
void i2c_config_clk(unsigned int value)//GPE3
{
	if(value == 0)
	{
		IO_OUTWM(PCONFE,SCL_CONFIG_MASK,0x0);
	}else{		
		IO_OUTWM(PCONFE,SCL_CONFIG_MASK,SCL_CONFIG_VALUE);
	}
}
void i2c_config_sda(unsigned int value)//GPG5
{
	if(value == 0)
	{
		IO_OUTWM(PCONFG,SDA_CONFIG_MASK,0x0);		
	}else{
		IO_OUTWM(PCONFG,SDA_CONFIG_MASK,SDA_CONFIG_VALUE);		
	}
}



void i2c_set_scl(unsigned int value)//GPE3
{
	unsigned int i = 1000;	
	IO_OUTWM(PDATE,SCL,value<<3);		
	delay_new(i);
}

void i2c_set_sda(unsigned int value)//GPG5
{
	unsigned int i = 1000;	
	IO_OUTWM(PDATG,SDA,value<<5);
	delay_new(i);
}

*/

void i2c_send_device(unsigned int read)
{
	int i = 0;
	for(i =0; i<7; i ++)
	{
		//i2c_set_sda((device_address>>(6-i))&1);
		i2c_set_sda((device_address>>(7-i))&1);
		i2c_set_scl(1);
		i2c_set_scl(0);		
	}	
	i2c_set_sda(read);
	i2c_set_scl(1);
	i2c_set_scl(0);
}

void i2c_send_data(unsigned int value)
{
	int i = 0;
	for(i =0; i<8; i ++)
	{
		i2c_set_sda((value>>(7-i))&1);
		i2c_set_scl(1);
		i2c_set_scl(0);		
	}
}


void i2c_receive_ack()
{
#if 1
	unsigned int ack = 0xff;
	unsigned int i = 100;
	i2c_config_sda(0);//input	
	delay_new(i);
	i2c_set_scl(1);	
	while(1)
	{
		ack = IO_INW(PDATC);
		if((ack&SDA) == 0) break;
	}
	i2c_set_scl(0);		
	i2c_config_sda(1);//output	
	delay_new(i);
#endif
}

unsigned int i2c_receive_data()
{
	unsigned int ack = 0; 
	unsigned int value = 0;
	
	i2c_config_sda(0);//input
	delay_new(0x100);
	
	i2c_set_scl(1);
	ack =  IO_INW(PDATC);
	value = value | ((ack&SDA)<<2);
	i2c_set_scl(0);

	i2c_set_scl(1);
	ack =  IO_INW(PDATC);
	value = value | ((ack&SDA)<<1);
	i2c_set_scl(0);

	i2c_set_scl(1);
	ack =  IO_INW(PDATC);
	value = value | (ack&SDA);
	i2c_set_scl(0); 

	i2c_set_scl(1);
	ack =  IO_INW(PDATC);
	value = value | ((ack&SDA)>>1);
	i2c_set_scl(0);

	i2c_set_scl(1);
	ack =  IO_INW(PDATC);
	value = value | ((ack&SDA)>>2);
	i2c_set_scl(0);

	i2c_set_scl(1);
	ack =  IO_INW(PDATC);
	value = value | ((ack&SDA)>>3);
	i2c_set_scl(0);

	i2c_set_scl(1);
	ack =  IO_INW(PDATC);
	value = value | ((ack&0x20)>>4);
	i2c_set_scl(0);
	
	i2c_set_scl(1);
	ack =  IO_INW(PDATC);
	value = value | ((ack&SDA)>>5);
	i2c_set_scl(0);
	return value;
}

void i2c_send_ack()
{
	i2c_config_sda(1);	
	i2c_set_sda(0);
	i2c_set_scl(1);
	i2c_set_scl(0);
}
void i2c_config(unsigned int value)
{
	i2c_config_clk(value);
	delay_new(1000);
	i2c_config_sda(value);
	delay_new(1000);
	if(value == 1)
	{
		i2c_set_scl(1);	
		i2c_set_sda(1);
	}
}

void i2c_start()
{	
	i2c_set_sda(0);
	i2c_set_scl(0);	
}

void i2c_stop()
{
	i2c_set_scl(1);
	i2c_set_sda(1);
}
void i2c_write_data(unsigned int address,unsigned int value)
{
	i2c_start();
	i2c_send_device(0);
	i2c_receive_ack();
	i2c_send_data(address);
	i2c_receive_ack();
	i2c_send_data(value);
	i2c_receive_ack();
	i2c_stop();
}

unsigned int i2c_read_data(unsigned int address)
{
	unsigned int value = 0xff;
	
	i2c_start();
	i2c_send_device(0);
	i2c_receive_ack();
	i2c_send_data(address);
	i2c_receive_ack();
	i2c_stop();
	
	i2c_start();
	i2c_send_device(1);
	i2c_receive_ack();
	value = i2c_receive_data();
	i2c_send_ack();
	i2c_stop();
	return value;
}

void config_pll()
{	
	i2c_config(1);
	i2c_write_data(0x04,0x5a);
	i2c_write_data(0x08,0x20);
	i2c_write_data(0x09,0x08);
	i2c_write_data(0x0a,0x0c);
	i2c_config(0);
}

void chip_reset()
{
	unsigned int delay = 100000;
	mercury_power(1);
	delay_new(delay);
	mercury_reset(1);
}
void check_pll_stable()
{
	unsigned int rtn = 0;
	while(rtn != 0xa55a)
	{
		//VCUST_IO_IN(0x16000c34,rtn);
		delay_new(0x1000);
	}
	//VCUST_IO_OUT(0x1600065c,0x3);	
}

void vrover_chip_init()
{	
	unsigned int i = 0x100000;
	mercury_power(0);
	mercury_reset(0);
	vrover_reset(1);
	mercury_power(1);
	config_pll();
	chip_reset();
	delay_new(i);
	check_pll_stable();
}

void normal_chip_init()
{
	unsigned int i = 0x100000;
	i2c_config(0);
	delay_new(i);
	mercury_power(1);
	delay_new(i);
	mercury_reset(1);
}
#if 0
void read_vrover_test()
{
	unsigned int ack = 0xff;
	unsigned int value = 0,value1 = 0,value2 = 0;
	//start
	//SCL GPE3
	//SDA GPG5
	//configscl();
	//set_scl(1);

	//configsda();
	//set_sda(1);
	set_sda(0);
	set_scl(0);	
	
	//send device address
	send_device(0x5E,0);	
	receive_ack();
	//send address 0x04
	send_data(0x04);
	receive_ack();
	stop();
	
	//start
	set_sda(0);
	set_scl(0);
	//start write address
	send_device(0x5E,1);	
	receive_ack();
	IO_OUTWM(PCONFG,0xC00,0x0);
	
	value = read_data();
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	set_sda(0);
	set_scl(1);
	set_scl(0);
	stop();

	set_sda(0);
	set_scl(0);	
	
	//send device address
	send_device(0x5E,0);	
	receive_ack();
	//send address 0x04
	send_data(0x08);
	receive_ack();
	stop();
	
	//start
	set_sda(0);
	set_scl(0);
	//start write address
	send_device(0x5E,1);	
	receive_ack();
	IO_OUTWM(PCONFG,0xC00,0x0);
	
	value1 = read_data();
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	set_sda(0);
	set_scl(1);
	set_scl(0);
	stop();

	set_sda(0);
	set_scl(0);	
	
	//send device address
	send_device(0x5E,0);	
	receive_ack();
	//send address 0x04
	send_data(0x09);
	receive_ack();
	stop();
	
	//start
	set_sda(0);
	set_scl(0);
	//start write address
	send_device(0x5E,1);	
	receive_ack();
	IO_OUTWM(PCONFG,0xC00,0x0);
	
	value2 = read_data();
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	set_sda(0);
	set_scl(1);
	set_scl(0);
	stop();

	set_sda(0);
	set_scl(0);	
	
	//send device address
	send_device(0x5E,0);	
	receive_ack();
	//send address 0x04
	send_data(0x0a);
	receive_ack();
	stop();
	
	//start
	set_sda(0);
	set_scl(0);
	//start write address
	send_device(0x5E,1);	
	receive_ack();
	IO_OUTWM(PCONFG,0xC00,0x0);
	
	value = read_data();
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	set_sda(0);
	set_scl(1);
	set_scl(0);
	stop();
	IO_OUTWM(PCONFG,0xC00,0x0);
	IO_OUTWM(PCONFE,0xc0,0x0);
	//set_scl(1);
	//set_sda(1);
	//delay_new();
	//IO_OUTWM(PCONFG,0x3000,0x1000);
	//IO_OUTWM(PDATG,0x40,0x0);	
}
#endif
int vrover_init_entry( void * context, void * para )
{
	unsigned int op = 0;
	TST_VROVER_PARA_T * parmeter = (TST_VROVER_PARA_T *) para;
	op = parmeter->op;

	switch(op){
		case OP_VROVER_CONFIG:
			{
				device_address = parmeter->data.device.address;				
			}
		case OP_VROVER_RESET:
			{
				unsigned int value  = parmeter->data.vrover_reset.value;
				vrover_reset(value);
				break;
			}
		case OP_MECURY_POWER:
			{
				unsigned int value  = parmeter->data.mercury_power.value;
				mercury_power(value);
				break;
				
			}
		case OP_MECURY_RESET:
			{
				unsigned int value  = parmeter->data.mercury_reset.value;
				mercury_reset(value);
				break;
			}		
		case OP_VROVER_SET:
			{
				break;
			}	
		case OP_VROVER_READ:
			{
				unsigned int value = 0;
				unsigned int address = parmeter->data.vrover_read.value;
				value = i2c_read_data(address);
//				PRINT("the value is 0x %x,\n\r",value);
				break;
			}
		case OP_VREVOR_WRITE:
			{
				unsigned int address = parmeter->data.vrover_write.address;
				unsigned int value = parmeter->data.vrover_write.value;
				i2c_write_data(address, value);
				break;
			}
		case OP_I2C_SET:
			{
				unsigned int value = parmeter->data.i2c_set.value;
				i2c_config(value);
				break;
			}
		case OP_VROVER_CHIP_INIT:
			{
				vrover_chip_init();
				break;
			}
		case OP_NORMAL_CHIP_INIT:
			{
				normal_chip_init();
				break;
			}
		default:
				break;
	}
	
	return 0;	
}
#else
void vrover_reset()
{
#if 0
	unsigned int ack = 0;
		

	//vrover reset GPG6
	//GPE4 power down   GPE5 reset
	IO_OUTWM(PCONFE,0x300,0x100);
	IO_OUTWM(PDATE,0x10,0x00);

	IO_OUTWM(PCONFE,0xc00,0x400);
	IO_OUTWM(PDATE,0x20,0x00);
	
	IO_OUTWM(PCONFG,0x3000,0x1000);
	IO_OUTWM(PDATG,0x40,0x40);

	IO_OUTWM(PCONFE,0xc0,0x40);
	IO_OUTWM(PDATE,GPE3,0x0);

	ack = IO_INW(PCONFG);


	IO_OUTWM(PCONFG,0xC00,0x400);

	ack = IO_INW(PCONFG);

	IO_OUTWM(PDATG,GPG5,0x0);
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	IO_OUTWM(PDATG,GPG5,0x20);
#else
	unsigned int i = 100000;
	IO_OUTWM(PCONFE,0x300,0x100);
	IO_OUTWM(PDATE,0x10,0x00);

	while(i) i--;
	IO_OUTWM(PCONFE,0xc00,0x400);
	IO_OUTWM(PDATE,0x20,0x00);
	i = 100000;
	while(i) i--;
	IO_OUTWM(PCONFG,0x3000,0x1000);
	IO_OUTWM(PDATG,0x40,0x40);
	i = 100000;
	while(i) i--;
#if 1 
	//power on
	IO_OUTWM(PCONFE,0x300,0x100);
	IO_OUTWM(PDATE,0x10,0x10);
	 i = 100000;
	while(i) i--;
#endif
#endif
}

void delay_new()
{
	unsigned int i = 1000;
	while(i) i --;
}

void set_scl(unsigned int value)
{
	delay_new();
	IO_OUTWM(PDATE,0x08,value<<3);		
}

void set_sda(unsigned int value)
{
	delay_new();
	IO_OUTWM(PDATG,0x20,value<<5);
}

void start()
{
	configscl();
	set_scl(1);
	configsda();
	delay_new();
	set_sda(1);
	set_sda(0);
	set_scl(0);	
}
void send_device(unsigned int value,unsigned int read)
{
	int i = 0;
	for(i =0; i<7; i ++)
	{
		set_sda((value>>(6-i))&1);
		set_scl(1);
		set_scl(0);		
	}	
	delay_new();
	set_sda(read);
	delay_new();
	set_scl(1);
	delay_new();
	set_scl(0);
}

void send_data(unsigned int value)
{
	int i = 0;
	for(i =0; i<8; i ++)
	{
		set_sda((value>>(7-i))&1);
		set_scl(1);
		set_scl(0);		
	}
}
void send_data_address(unsigned int address,unsigned int value)
{
	send_device(0x5E,0);
	send_data(address);
	send_data(value);
}
void receive_ack()
{
	unsigned int ack = 0xff;
	IO_OUTWM(PCONFG,0xC00,0x0);	
	delay_new();
	set_scl(1);
	delay_new();
	while(1)
	{
		ack = IO_INW(PDATG);
		if((ack&0x20) == 0) break;
	}
	set_scl(0);		
	IO_OUTWM(PCONFG,0xC00,0x400);
	delay_new();
}

unsigned int  read_data()
{
	int i = 0;
	unsigned int ack = 0; 
	unsigned int value = 0;
	//for(i = 0;i<8;i++)
	//{
		set_scl(1);
		ack =  IO_INW(PDATG);
		value = value | ((ack&0x20)<<2);
		set_scl(0);

		set_scl(1);
		ack =  IO_INW(PDATG);
		value = value | ((ack&0x20)<<1);
		set_scl(0);

		set_scl(1);
		ack =  IO_INW(PDATG);
		value = value | (ack&0x20);
		set_scl(0); 

		set_scl(1);
		ack =  IO_INW(PDATG);
		value = value | ((ack&0x20)>>1);
		set_scl(0);

		set_scl(1);
		ack =  IO_INW(PDATG);
		value = value | ((ack&0x20)>>2);
		set_scl(0);

		set_scl(1);
		ack =  IO_INW(PDATG);
		value = value | ((ack&0x20)>>3);
		set_scl(0);

		set_scl(1);
		ack =  IO_INW(PDATG);
		value = value | ((ack&0x20)>>4);
		set_scl(0);
	
		set_scl(1);
		ack =  IO_INW(PDATG);
		value = value | ((ack&0x20)>>5);
		set_scl(0);
	//}	
	return value;
}
void stop()
{
	set_scl(1);
	set_sda(1);
}
void config_pll()
{	
	//start
	//SCL GPE3
	//SDA GPG5
	start();
	
	send_device(0x5E,0);	
	receive_ack();
	//receive ack
	send_data(0x04);
	receive_ack();		
	send_data(0x5a);
	receive_ack();
	stop();

	set_sda(0);
	set_scl(0);
	send_device(0x5E,0);	
	receive_ack();
	//config 08,send address 0x08
	send_data(0x08);
	receive_ack();		
	send_data(0x20);
	receive_ack();	
	stop();

	set_sda(0);
	set_scl(0);
	send_device(0x5E,0);	
	receive_ack();	
	//config 09,send address 0x09
	send_data(0x09);
	receive_ack();		
	send_data(0x08);
	receive_ack();	
	stop();

	set_sda(0);
	set_scl(0);
	send_device(0x5E,0);	
	receive_ack();
	//config 0a,send address 0x0a
	send_data(0x0a);
	receive_ack();		
	send_data(0x0c);
	receive_ack();	
	stop();
#if 0
	IO_OUTWM(PCONFG,0xC00,0x0);
	IO_OUTWM(PCONFE,0xc0,0x0);
#endif	
}

void chip_reset()
{
	unsigned int i = 100000;
	//IO_OUTWM(PCONFE,0x300,0x100);
	//IO_OUTWM(PDATE,0x10,0x10);
	while(i) i--;
#if 0
//power down	
	IO_OUTWM(PCONFE,0x300,0x100);
	IO_OUTWM(PDATE,0x10,0x0);
	delay_new();
	i = 100000;
	while(i) i--;	
//power on
	IO_OUTWM(PCONFE,0x300,0x100);
	IO_OUTWM(PDATE,0x10,0x10);
	delay_new();
	i = 100000;
	while(i) i--;
#endif
//reset high
	IO_OUTWM(PCONFE,0xC00,0x400);
	IO_OUTWM(PDATE,0x20,0x20);
}

void read_vrover_test()
{
	unsigned int ack = 0xff;
	unsigned int value = 0,value1 = 0,value2 = 0;
	//start
	//SCL GPE3
	//SDA GPG5
	//configscl();
	//set_scl(1);

	//configsda();
	//set_sda(1);
	set_sda(0);
	set_scl(0);	
	
	//send device address
	send_device(0x5E,0);	
	receive_ack();
	//send address 0x04
	send_data(0x04);
	receive_ack();
	stop();
	
	//start
	set_sda(0);
	set_scl(0);
	//start write address
	send_device(0x5E,1);	
	receive_ack();
	IO_OUTWM(PCONFG,0xC00,0x0);
	
	value = read_data();
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	set_sda(0);
	set_scl(1);
	set_scl(0);
	stop();

	set_sda(0);
	set_scl(0);	
	
	//send device address
	send_device(0x5E,0);	
	receive_ack();
	//send address 0x04
	send_data(0x08);
	receive_ack();
	stop();
	
	//start
	set_sda(0);
	set_scl(0);
	//start write address
	send_device(0x5E,1);	
	receive_ack();
	IO_OUTWM(PCONFG,0xC00,0x0);
	
	value1 = read_data();
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	set_sda(0);
	set_scl(1);
	set_scl(0);
	stop();

	set_sda(0);
	set_scl(0);	
	
	//send device address
	send_device(0x5E,0);	
	receive_ack();
	//send address 0x04
	send_data(0x09);
	receive_ack();
	stop();
	
	//start
	set_sda(0);
	set_scl(0);
	//start write address
	send_device(0x5E,1);	
	receive_ack();
	IO_OUTWM(PCONFG,0xC00,0x0);
	
	value2 = read_data();
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	set_sda(0);
	set_scl(1);
	set_scl(0);
	stop();

	set_sda(0);
	set_scl(0);	
	
	//send device address
	send_device(0x5E,0);	
	receive_ack();
	//send address 0x04
	send_data(0x0a);
	receive_ack();
	stop();
	
	//start
	set_sda(0);
	set_scl(0);
	//start write address
	send_device(0x5E,1);	
	receive_ack();
	IO_OUTWM(PCONFG,0xC00,0x0);
	
	value = read_data();
	
	IO_OUTWM(PCONFG,0xC00,0x400);
	set_sda(0);
	set_scl(1);
	set_scl(0);
	stop();
	IO_OUTWM(PCONFG,0xC00,0x0);
	IO_OUTWM(PCONFE,0xc0,0x0);
	//set_scl(1);
	//set_sda(1);
	//delay_new();
	//IO_OUTWM(PCONFG,0x3000,0x1000);
	//IO_OUTWM(PDATG,0x40,0x0);	
}

int vrover_init_entry( void * context, void * para )
{
	unsigned int rtn = 0;
	unsigned int i = 0x100000;
	delay_new();
	vrover_reset();
	delay_new();
	//
	config_pll();
	read_vrover_test();


	chip_reset();

	delay_new();
	delay_new();
	delay_new();
	delay_new();
	delay_new();
	delay_new();
	delay_new();
	delay_new();
	delay_new();
	delay_new();
	delay_new();

	
	while(rtn != 0xa55a)
	{
		VCUST_IO_IN(0x16000c34,rtn);
		delay_new();
	}

	VCUST_IO_OUT(0x1600065c,0x3);
	
	return 0;
}
#endif


int normal_init_entry( void * context, void * para )
{
	unsigned int rtn = 0;
	unsigned int i = 0x100000;
	
	IO_OUTWM(PCONFG,0xC00,0x0);
	IO_OUTWM(PCONFE,0xc0,0x0);
	while(i) i--;
	IO_OUTWM(PCONFE,0x300,0x100);
	IO_OUTWM(PDATE,0x10,0x10);
	i = 100000;
	while(i) i--;
	IO_OUTWM(PCONFE,0xc00,0x400);
	IO_OUTWM(PDATE,0x20,0x20);
	i = 100000;
	while(i) i--;
	return rtn;
}

